/*
* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "b2Collision.h"
#include "Shapes/b2CircleShape.h"
#include "Shapes/b2EdgeShape.h"
#include "Shapes/b2PolygonShape.h"

// This implement 2-sided edge vs circle collision.
void b2CollideEdgeAndCircle(b2Manifold* manifold, const b2EdgeShape* edge, const b2XForm& xf1, const b2CircleShape* circle, const b2XForm& xf2)
{
	manifold->m_pointCount = 0;
	b2Vec2 cLocal = b2MulT(xf1, b2Mul(xf2, circle->m_p));
	b2Vec2 normal = edge->m_normal;
	b2Vec2 v1 = edge->m_v1;
	b2Vec2 v2 = edge->m_v2;
	float32 radius = edge->m_radius + circle->m_radius;

	// Barycentric coordinates
	float32 u1 = b2Dot(cLocal - v1, v2 - v1);
	float32 u2 = b2Dot(cLocal - v2, v1 - v2);

	if (u1 <= 0.0f)
	{
		// Behind v1
		if (b2DistanceSquared(cLocal, v1) > radius * radius)
		{
			return;
		}

		manifold->m_pointCount = 1;
		manifold->m_type = b2Manifold::e_faceA;
		manifold->m_localPlaneNormal = cLocal - v1;
		manifold->m_localPlaneNormal.Normalize();
		manifold->m_localPoint = v1;
		manifold->m_points[0].m_localPoint = circle->m_p;
		manifold->m_points[0].m_id.key = 0;
	}
	else if (u2 <= 0.0f)
	{
		// Ahead of v2
		if (b2DistanceSquared(cLocal, v2) > radius * radius)
		{
			return;
		}

		manifold->m_pointCount = 1;
		manifold->m_type = b2Manifold::e_faceA;
		manifold->m_localPlaneNormal = cLocal - v2;
		manifold->m_localPlaneNormal.Normalize();
		manifold->m_localPoint = v2;
		manifold->m_points[0].m_localPoint = circle->m_p;
		manifold->m_points[0].m_id.key = 0;
	}
	else
	{
		float32 separation = b2Dot(cLocal-v1, normal);
		if((-radius>separation) || (separation>radius))
		{
			return;	
		}

		manifold->m_pointCount = 1;
		manifold->m_type = b2Manifold::e_faceA;
		manifold->m_localPlaneNormal = separation < 0.0f ? -normal : normal;
		manifold->m_localPoint = 0.5f * (v1 + v2);
		manifold->m_points[0].m_localPoint = circle->m_p;
		manifold->m_points[0].m_id.key = 0;
	}
}

#if 1

//Polygon versus 2-sided edge
void b2CollidePolyAndEdge(b2Manifold* manifold, const b2PolygonShape* polygon, const b2XForm& xf1, const b2EdgeShape* edge, const b2XForm& xf2 )
{
	b2PolygonShape polygonB;
	polygonB.SetAsEdge(edge->m_v1, edge->m_v2);

	b2CollidePolygons(manifold, polygon, xf1, &polygonB, xf2);
}

#else
//XY not porting this part.
#endif